package ftn.gitis.lidar.filtering;

import java.awt.Graphics;
import java.io.BufferedReader;
import java.io.File;
import java.io.FileReader;
import java.io.FileWriter;
import java.io.IOException;
import java.io.PrintWriter;
import java.util.ArrayList;
import java.util.Iterator;


import delaunay_triangulation.Delaunay_Triangulation;
import delaunay_triangulation.Triangle_dt;

/**
 * Class that contains all points collected by LIDAR for an area of interest. 
 * @author Paja
 *
 */
public class LidarPoints {

	/**
	 * Array of LIDAR points.
	 */
	private ArrayList<LidarPoint> points;
	
	/**
	 * Array of LIDAR points which falls on terrain.
	 */
	private ArrayList<LidarPoint> groundPoints;
	
	/**
	 * Bounding box of LIDAR points in XY plane.
	 */
	private BoundingBox boundingBox;
	
	private Graphics drawPanel;
	
	/**
	 * Default constructor. Sets points to <b>null</b>.
	 */
	public LidarPoints() {
		super();
		points = null;
		groundPoints = null;
		boundingBox = null;
	}
	
	/**
	 * Creates LIDAR points by loading them from a specified file. 
	 * @param file
	 */
	public LidarPoints(File file) {
		super();
		load(file);
	}
	
	/**
	 * Returns LiDAR points.
	 * @return
	 */
	public ArrayList<LidarPoint> getPoints() {
		return points;
	}
	
	/**
	 * Sets LiDAR points.
	 * @param points
	 */
	public void setPoints(ArrayList<LidarPoint> points) {
		this.points = points;
	}
	
	public Graphics getDrawPanel() {
		return drawPanel;
	}

	public void setDrawPanel(Graphics drawPanel) {
		this.drawPanel = drawPanel;
	}

	/**
	 * Loads LIDAR points from an text file where each line contains 
	 * XYZ coordinates of a point delimited with space.
	 * @param file a file the points are read from.
	 */
	public void load(File file) {
		try {
			BufferedReader in = new BufferedReader(new FileReader(file));
			
			String line;
			int i = 0;
			double x;
			double y;
			double z;
			double minx = Double.MAX_VALUE;
			double miny = Double.MAX_VALUE;
			double maxx = -Double.MAX_VALUE;
			double maxy = -Double.MAX_VALUE;
			
			points = new ArrayList<LidarPoint>();
			int classification = 1; //all point are initially declared unclassified
			while((line = in.readLine()) != null) {
				i++;
				try {
					x = Double.parseDouble(line.substring(0, line.indexOf(' ')));
					y = Double.parseDouble(line.substring(line.indexOf(' ') + 1, line.lastIndexOf(' ')));
					z = Double.parseDouble(line.substring(line.lastIndexOf(' ')));
					points.add(new LidarPoint(x, y, z, classification));
					
					if(x < minx) minx = x;
					else if (x > maxx) maxx = x;
					if(y < miny) miny = y;
					else if(y > maxy) maxy = y;
					
				} catch (NumberFormatException ex){
					System.out.println("Line" + i + "is omited.");
				}
			}
			
			boundingBox = new BoundingBox(minx, maxy, maxx, miny);
			in.close();
			
			System.out.println("Total nuber of " + points.size() +  " are loaded.");
		} catch(IOException ex) {
			ex.printStackTrace();
		}	
	}
	
	/**
	 * Saves the LIDAR points into text file formated as CXYZ, delimited with space.
	 * @param file
	 */
	public void save(File file) {
		try {
			PrintWriter out = new PrintWriter(new FileWriter(file));
			for(int i=0; i<points.size(); i++) {
				out.println(points.get(i).getClassification() + " " + points.get(i).x() + " " + points.get(i).y() + " " + points.get(i).z());
			}
			out.close();
		} catch (IOException ex) {
			ex.printStackTrace();
		}
		
	}
	
	public void classifyGroundPoints(ClassificationParameters params) {
		LidarPoint[][] initialPoints;
		LidarPoint[] initialPointsArray;
		int numOfInitials = 0;
		Delaunay_Triangulation triangulation;
		int foundGroundPoints = 0;
		
		/* Find initial ground points */
		
		/* The initial selection area will be divided into squares of dimensions maxBuildingSize * maxBuildingSize */
		int numOfRows = (int) Math.ceil(boundingBox.getHeight() / params.getMaxBuildingSize()) + 1;
		int numOfCols = (int) Math.ceil(boundingBox.getWidth() / params.getMaxBuildingSize()) + 1;

		/* Boundaries of the selection area*/
		double stretchX = (numOfCols * params.getMaxBuildingSize() - boundingBox.getWidth()) / 2;
		double stretchY = (numOfRows * params.getMaxBuildingSize() - boundingBox.getHeight()) / 2;
		double saUlx = boundingBox.getUlx() - stretchX;
		double saUly = boundingBox.getUly() + stretchY;
		double saLrx = boundingBox.getLrx() + stretchX;
		double saLry = boundingBox.getLry() - stretchY; 
		
		initialPoints = new LidarPoint[numOfRows][numOfCols];
		for(int c=0; c < numOfCols; c++) 
			for(int r=0; r < numOfRows; r++) 
				initialPoints[r][c] = null;
		
		/* Find initial points */
		initialPoints = new LidarPoint[numOfRows][numOfCols];
		/* Iterates through all of points to find lowest for each selection area*/
		for(int i=0; i < points.size(); i++) {
			int indexX = (int)Math.floor((points.get(i).x() - saUlx) / params.getMaxBuildingSize());
			int indexY = (int)Math.floor((saUly - points.get(i).y()) / params.getMaxBuildingSize());
			
			if(initialPoints[indexY][indexX] == null) {
				initialPoints[indexY][indexX] = points.get(i);
				numOfInitials++;
			}
			else if (initialPoints[indexY][indexX].z() > points.get(i).z())
				initialPoints[indexY][indexX] = points.get(i);
		}
		
		
		
		/* Display initial points */
		for(int i=0; i < initialPoints.length; i++) {
			for(int j=0; j < initialPoints[0].length; j++) {
				if(initialPoints[i][j] == null)
					System.out.print("null ");
				else
					System.out.print(initialPoints[i][j].z() + " ");
			}
			System.out.println();
		}
		
		/* Set classification of initial points as Ground and prepare initialPointsArray for triangulation */
		
		int k=0; //index of initialPointsArray
		int lrRow = 0;
		int lrCol = 0;
		int urRow = initialPoints.length - 1;
		int urCol = 0;
		int llRow = 0;
		int llCol = initialPoints[0].length - 1;
		int ulRow = initialPoints.length - 1;
		int ulCol = initialPoints[0].length - 1;
		initialPointsArray = new LidarPoint[numOfInitials + 4];
		groundPoints = new ArrayList<LidarPoint>();
		for(int i=0; i<initialPoints.length; i++)
			for(int j=0; j<initialPoints[0].length; j++) 
				if(initialPoints[i][j] != null) {
					initialPoints[i][j].setClassification(2);
					groundPoints.add(initialPoints[i][j]);
					initialPointsArray[k] = initialPoints[i][j];
					
					int tempRow = i;
					int tempCol = j;
					
					if((tempRow * tempRow + tempCol*tempCol) < (ulRow * ulRow + ulCol*ulCol)) {
						ulRow = tempRow;
						ulCol = tempCol;
					}
					
					tempRow = i;
					tempCol = initialPoints[0].length - 1 - j;
					
					if((tempRow * tempRow + tempCol*tempCol) < (urRow * urRow + urCol*urRow)) {
						urRow = tempRow;
						urCol = tempCol;
					}
					
					tempRow = initialPoints.length - 1 - i;
					tempCol = initialPoints[0].length - 1 - j;
					
					if((tempRow * tempRow + tempCol*tempCol) < (lrRow * lrRow + lrCol*lrRow)) {
						lrRow = tempRow;
						lrCol = tempCol;
					}
					
					tempRow = initialPoints.length - 1 - i;
					tempCol = j;
					
					if((tempRow * tempRow + tempCol*tempCol) < (llRow * llRow + llCol*llRow)) {
						llRow = tempRow;
						llCol = tempCol;
					}
					
					k++;
				}
		initialPointsArray[numOfInitials - 4] = new LidarPoint(saUlx, saUly, initialPoints[ulRow][ulCol].z());
		initialPointsArray[numOfInitials - 3] = new LidarPoint(saLrx, saUly, initialPoints[urRow][urCol].z());
		initialPointsArray[numOfInitials - 2] = new LidarPoint(saLrx, saLry, initialPoints[lrRow][lrCol].z());
		initialPointsArray[numOfInitials - 1] = new LidarPoint(saUlx, saLry, initialPoints[llRow][llCol].z());
		
		/* Calculate initial TIN */
		triangulation = new Delaunay_Triangulation(initialPointsArray);
		
		/* Classify points */
		LidarPoint[] newPoints;
		Iterator<Triangle_dt> triangIter; 
		ArrayList<Triangle_dt> currentTriangles; 
		Triangle_dt tempTriangle;
		double iterAngleWeight; //iteration angle gets smaller when triangle is small 
		int t;
		int iteration = 0;
		boolean newPointsExists;
		boolean newMirrorsExists;
		do {
			iteration++;
			System.out.println("Iteration: " + iteration);
			newPointsExists = false;
			newMirrorsExists = false;
			triangIter = triangulation.trianglesIterator();
			currentTriangles = new ArrayList<Triangle_dt>();
			while(triangIter.hasNext())
				currentTriangles.add(triangIter.next());
			newPoints = new LidarPoint[currentTriangles.size()];
			for(int i=0; i<points.size(); i++) 
				if(points.get(i).getClassification() == 1) {
					tempTriangle = triangulation.find(points.get(i));
					if(!tempTriangle.isHalfplane()) {
						t = currentTriangles.indexOf(tempTriangle);
					
						/* Reduction of iteration angle */
						iterAngleWeight = TriangleUtils.getLongestEdgeLength(tempTriangle) / 5.0D; //add to classification parameters longest egde parameter
						if(iterAngleWeight > 1.0D)
							iterAngleWeight = 1.0D;
						else
							iterAngleWeight *= iterAngleWeight;
	
						/* Check a point against classification parameters */
						if((points.get(i).getMaxAngleToTriangle(tempTriangle) < params.getIterationAngle() * iterAngleWeight) 
							&& (points.get(i).getDistanceFromTriangle(tempTriangle) < params.getIterationDistance())) {
							if(newPoints[t] == null) {
								newPoints[t] = points.get(i);
								newPointsExists = true;
							}
							else if ((points.get(i).getMaxAngleToTriangle(tempTriangle) < newPoints[t].getMaxAngleToTriangle(tempTriangle))
									&& points.get(i).getDistanceFromTriangle(tempTriangle) < newPoints[t].getDistanceFromTriangle(tempTriangle)) {
								newPoints[t] =  points.get(i);
								newPointsExists = true;
							}
						}
					}
				}
			
			/* Inserts new points into triangulation */
			
			int n = 0;
			for(int i=0; i<newPoints.length; i++)
				if(newPoints[i] != null) {
					newPoints[i].setClassification(2);
					triangulation.insertPoint(newPoints[i]);
					groundPoints.add(newPoints[i]);
					n++;
				}

			foundGroundPoints += n;
			System.out.println("Found new points: " + foundGroundPoints);
			
		} while(newPointsExists || newMirrorsExists);
		
	}
	
}
